/* 
 *  <copyright file="OrientationCmp.cs" company="Wesley Jowitt">
 *  
 *  SobrietyEngine: Copyright (c) 2011 Wesley Jowitt
 *  
 *  This software is distributed under a software license.
 *   * As of the creation time of this file, the license in use is the Microsoft Permissive License (Ms-PL) 
 *    * The license can be viewed at:
 *     * License.Txt in the root folder of this project
 *      * http://sobrietyengine.codeplex.com/license
 *       * http://www.microsoft.com/opensource/licenses.mspx
 *       
 *  </copyright>
 *   */

using System;


using Microsoft.Xna.Framework;

namespace SobrietyEngine.Components.Common
{
    public class OrientationCmp : BaseComponent, IOrientation
    {
        public Vector3 GlobalForward { get; private set; }
        public Vector3 GlobalUp { get; private set; }
        public Vector3 GlobalRight { get; private set; }
        
        public Vector3 Forward { get; private set; }
        public Vector3 Up { get; private set; }
        public Vector3 Right { get; private set; }

        public Vector3 ForwardPerpToGlobalUp { get; private set; }

        public Matrix RotationMatrix { get; private set; }

        //lockup doesnt work quite as expectedyet
        public Boolean LockUp { get; set; }
        //rotatin origin does not work as ofyet
        public Vector3 RotationOrigin { get; private set; }

        public Quaternion Orientation { get; private set; }

        IPosition positionCmp;

        public OrientationCmp()
            : base()
        {
            Orientation = Quaternion.Identity;
            GlobalForward = Vector3.Forward;
            GlobalUp = Vector3.Up;
            GlobalRight = Vector3.Right;
            RotationOrigin = Vector3.Zero;

            LockUp = false;
            UpdateProps();
            LockUp = true;
        }

        public OrientationCmp(Vector3 forward)
            : this()
        {
            Set(forward);
        }

        override public Type[] SupportedInterfaces()
        {
            return new Type[] { typeof(IOrientation) };
        }

         override public Boolean InitComponent()
        {
            base.InitComponent();
            positionCmp = Actor.Get<IPosition>();
            return true;
        }
        
        public void Set(float yaw)
        {
            Set(new Vector2((float)Math.Cos(yaw), (float)Math.Sin(yaw)));
        }

        public void Set(Vector2 dir)
        {
            Set(new Vector3(dir, 0));
        }

        public void Set(Vector3 dir)
        {
            dir.Normalize();
            Matrix m = Matrix.CreateLookAt(Vector3.Zero, dir, Up);
            Orientation = Quaternion.CreateFromRotationMatrix(m);
            UpdateProps();
        }

        public void Reset()
        {
            //force update all properties
            Orientation = Quaternion.Identity;

            Boolean l = LockUp;
            LockUp = false;
            UpdateProps();
            LockUp = l;
        }

        public void SetWorldAxis(Vector3 forward, Vector3 up, Vector3 right)
        {
            Orientation = Quaternion.Identity;

            GlobalForward = forward;
            GlobalUp = up;
            GlobalRight = right;
            //force update all properties
            Boolean l = LockUp;
            LockUp = false;
            UpdateProps();
            LockUp = l;
        }

        public void Set(Vector3 forward, Vector3 up)
        {
            Matrix m = Matrix.CreateLookAt(Vector3.Zero, forward, up);
            Orientation = Quaternion.CreateFromRotationMatrix(m);
            UpdateProps();
        }

        public void Set(float yaw, float pitch, float roll)
        {
            Orientation = Quaternion.Identity;
            WorldAxisYawPitchRoll(yaw, pitch, roll);
        }

        public void SetLookAt(Vector3 lookAtPoint)
        {
            Vector3 dir = (lookAtPoint - positionCmp.PositionV3);
            dir.Normalize();
            Matrix m = Matrix.CreateLookAt(Vector3.Zero, dir, Up);
            Orientation = Quaternion.CreateFromRotationMatrix(m);
            UpdateProps();
        }

        public void ToAxisAngle(Vector3 axis, float angle)
        {
            if (axis == Vector3.Zero)
                return;
            Orientation = Quaternion.CreateFromAxisAngle(axis, angle);
            UpdateProps();
        }

        public void ByAxisAngle(Vector3 axis, float angle)
        {
            if (angle == 0 || axis == Vector3.Zero)
                return;
            Quaternion rot = Quaternion.CreateFromAxisAngle(axis, angle);
            Orientation *= rot;
            UpdateProps();
        }

        public void WorldAxisRoll(float amt)
        {
            ByAxisAngle(GlobalForward, amt);
        }

        public void WorldAxisYaw(float amt)
        {
            ByAxisAngle(GlobalUp, amt);
        }

        public void WorldAxisPitch(float amt)
        {
            ByAxisAngle(GlobalRight, amt);
        }

        public void WorldAxisYawPitchRoll(float yaw, float pitch, float roll)
        {
            WorldAxisYaw(yaw);
            WorldAxisPitch(pitch);
            WorldAxisRoll(roll);
        }

        public void RelativeAxisRoll(float amt)
        {
            ByAxisAngle(Forward, amt);
        }

        public void RelativeAxisYaw(float amt)
        {
            ByAxisAngle(Up, amt);
        }

        public void RelativeAxisPitch(float amt)
        {
            ByAxisAngle(Right, amt);
        }

        public void RelativeAxisYawPitchRoll(float yaw, float pitch, float roll)
        {
            RelativeAxisYaw(yaw);
            RelativeAxisPitch(pitch);
            RelativeAxisRoll(roll);
        }


        public float GetRadiansXY()
        {
            return (float)Math.Atan2((double)Forward.Y, (double)Forward.X);
        }        

        private void UpdateProps()
        {
            Orientation.Normalize();

            Quaternion q = Orientation;
            q.Normalize();
            q = Quaternion.Inverse(q);

            Forward = Vector3.Transform(GlobalForward, q);
            Forward.Normalize();

            Right = Vector3.Transform(GlobalRight, q);
            Right.Normalize();

            if (!LockUp)
            {
                Up = Vector3.Transform(GlobalUp, q);
                Up.Normalize();
            }

            ForwardPerpToGlobalUp = Vector3.Cross(GlobalUp, Right);
            ForwardPerpToGlobalUp.Normalize();
            
            RotationMatrix = Matrix.CreateFromQuaternion(Orientation);
        }
    }
}